﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.IO;


namespace KSASupport.Imaging
{
    /// <summary>
    /// Provide ways to visualize the depthimage data from the Kinect
    /// </summary>
    public static class KinectImageConversion
    {
        const int RedIndex = 2;
        const int GreenIndex = 1;
        const int BlueIndex = 0;

        /// <summary>
        /// Takes in a depth value, and returns a 0-255 value to represent it
        /// </summary>
        /// <param name="distance">distance value from Kinect</param>
        /// <returns></returns>
        public static byte CalculateIntensityFromDepth(int distance)
        {
            //formula for calculating monochrome intensity for histogram
            byte val = (byte)(255 - (255 * Math.Max(distance, 255) / (255)));
            val = (byte)(255 - (255 * Math.Max(distance - 400, 0)  / (1000)));
            return val;
        }

        /// <summary>
        /// Assigns a number to an  image 'floating over the table'
        /// </summary>
        /// <param name="distance">sisance to convert</param>
        /// <returns></returns>
        public static byte CalculateIntensityFromDepth2(int distance)
        {
            //formula for calculating monochrome intensity for histogram
            return (byte)(255 - (255 * Math.Max(distance , 20) / (255)));
        }
        

        /// <summary>
        /// Calculate a false color image from an int array, such as the threshold image
        /// </summary>
        /// <param name="depthPixels">array of depth pixels to work with</param>
        /// <param name="Width">width of the image</param>
        /// <param name="Height">Height of the image</param>
        /// <param name="Cutoff">A cutoff vaue, which should be ignored if the depth is higher</param>
        /// <param name="tolerance">how high from the cutoff should be included</param>
        /// <returns></returns>
        public static byte[] ConvertintToByteImage(int[] depthPixels, int Width, int Height,int Cutoff,int tolerance)
        {
            if (depthPixels == null)
            {
                return new byte[Width * Height * 4];
            }
            byte[] DepthImage = new byte[depthPixels.Length * 4];


            Parallel.For(0, Height, yIndex =>
            {
                int heightOffset = yIndex * Width;
                for (int x = 0; x < Width; x++)
                {
                    int index = (x + heightOffset) * 4;
                    int CalibrationIndex = x + heightOffset;

                    int distance = depthPixels[CalibrationIndex];
                    if (distance > 0)
                    {
                        byte value = CalculateIntensityFromDepth2(distance);
                        if ((distance < Cutoff) && (distance > tolerance))
                        {
                            DepthImage[index + GreenIndex] = value;
                        }
                        else
                        {
                            DepthImage[index + GreenIndex] = value;
                            DepthImage[index + BlueIndex] = value;
                            DepthImage[index + GreenIndex] = value;
                        }
                    }
                    else
                    {
                        DepthImage[index + RedIndex] = 0;
                        DepthImage[index + GreenIndex] = 0;
                        DepthImage[index + BlueIndex] = 0;
                    }

                }
            });

            return DepthImage;
        }

        /// <summary>
        /// Takes in a short array, and returns it as a false color byte array
        /// </summary>
        /// <param name="depthPixels"></param>
        /// <returns></returns>
        public static byte[] ConvertShortToByteImage(short[] depthPixels,int Width,int Height)
        {
            if (depthPixels == null)
            {
                return new byte[Width * Height * 4];
            }
            byte[] DepthImage = new byte[depthPixels.Length * 4];

            Parallel.For(0, Height, yIndex =>
            {
                int heightOffset = yIndex * Width;
                for (int x = 0; x < Width; x++)
                {
                    int index = (x + heightOffset) * 4;
                    int CalibrationIndex = x + heightOffset;

                    int distance = depthPixels[CalibrationIndex];
                    if (distance > 0)
                    {
                        byte value = CalculateIntensityFromDepth(distance);
                        DepthImage[index + RedIndex] = value;
                        DepthImage[index + GreenIndex] = value;
                        DepthImage[index + BlueIndex] = value;
                    }
                    else
                    {
                        DepthImage[index + RedIndex] = 255;
                        DepthImage[index + GreenIndex] = 0;
                        DepthImage[index + BlueIndex] = 0;
                    }
                    
                }
            });

            return DepthImage;
        }

        /// <summary>
        /// Takes in a short array, and returns it as a false color byte array
        /// </summary>
        /// <param name="depthPixels"></param>
        /// <returns></returns>
        public static byte[] ConvertShortToByteImage(short[] depthPixels, int Width, int Height,int value)
        {
            if (depthPixels == null)
            {
                return new byte[Width * Height * 4];
            }
            byte[] DepthImage = new byte[depthPixels.Length * 4];

            Parallel.For(0, Height, yIndex =>
            {
                int heightOffset = yIndex * Width;
                for (int x = 0; x < Width; x++)
                {
                    int index = (x + heightOffset) * 4;
                    int CalibrationIndex = x + heightOffset;

                    int distance = depthPixels[CalibrationIndex];
                    if (distance > 0)
                    {
                        //byte value = CalculateIntensityFromDepth(distance);
                        DepthImage[index + RedIndex] = (byte)value;
                        DepthImage[index + GreenIndex] = (byte)value;
                        DepthImage[index + BlueIndex] = (byte)value;
                    }
                    else
                    {
                        DepthImage[index + RedIndex] = 120;
                        DepthImage[index + GreenIndex] = 0;
                        DepthImage[index + BlueIndex] = 0;
                    }

                }
            });

            return DepthImage;
        }

        /// <summary>
        /// Simple 'image capture' method
        /// </summary>
        /// <param name="depthArray"></param>
        /// <param name="fileName"></param>
        static public void SaveIntArrayToFile(int[] depthArray, string fileName)
        {
            if (depthArray == null)
                return;

            StreamWriter sw = new StreamWriter(fileName,false);
            for (int i = 0; i < depthArray.Length; i++)
            {
                sw.Write(depthArray[i].ToString() + " ");
            }
            sw.Flush();
            sw.Close();
            sw.Dispose();
        }
    }
}
